Route planning of mobile robot based on improved RRT star and TEB algorithm

This paper presents a fusion algorithm based on the enhanced RRT* TEB algorithm. The enhanced RRT* algorithm is utilized for generating an optimal global path. Firstly, proposing an adaptive sampling function and extending node bias to accelerate global path generation and mitigate local optimality. Secondly, eliminating path redundancy to minimize path length. Thirdly, imposing constraints on the turning angle of the path to enhance path smoothness. Conducting kinematic modeling of the mobile robot and optimizing the TEB algorithm to align the trajectory with the mobile robot's kinematics. The integration of these two algorithms culminates in the development of a fusion algorithm. Simulation and experimental results demonstrate that, in contrast to the traditional RRT* algorithm, the enhanced RRT* algorithm achieves a 5.8% reduction in path length and a 62.5% decrease in the number of turning points. Utilizing the fusion algorithm for path planning, the mobile robot generates a superior, seamlessly smooth global path, adept at circumventing obstacles. Furthermore, the local trajectory meticulously conforms to the kinematic constraints of the mobile robot.


Platform hardware introduction
The mobile robot comprises a depth camera, laser radar, main control unit and robot chassis.The main control unit is built around a Raspberry Pi, incorporating an STM32 chip.The robot chassis is constructed with two servo motors, two driving wheels, and an omnidirectional wheel, as depicted in Fig. 1.

Platform system introduction
The platform system integrates the ROS operating system into Raspberry Pi, with key components encompassing environment awareness, data processing and path navigation.ROS, widely recognized as the most extensively used open-source robot software platform, greatly improves the efficiency of robot development.The laser radar collects environmental data, while the Raspberry Pi and STM32 chip work together to process it and plan an optimal, obstacle-free path.The servo motor is controlled to start tire rotation, helping the mobile robot move along the planned path, as illustrated in Fig. 2.

Kinematic model of the mobile robot
This paper conducts kinematic modeling for the mobile robot 29 .Within the world coordinate system (x w , y w ), the state vector s = [x, y, θ] denotes the current position and pose information of the mobile robot.This is depicted in Fig. 3.   www.nature.com/scientificreports/Given that the mobile robot operates on a two-wheeled differential system, each drive wheel is independently powered by a dedicated motor.Here, r represents the radius of the driving wheel, while w l (t) and w r (t) denote the angular speed of the left and right driving wheels, respectively.The linear speed of the left and right drive wheels is expressed as v l (t) and v r (t).where t represents the turning time period, p is half of the wheelbase of the driving wheel.Consequently, the local coordinate system for the mobile robot positions the y-axis along the horizontal direction of its two driving wheels, with the x-axis aligned with its moving direction.The speed of the mobile robot, denoted as v(t).Speed of the mobile robot v(t) = (v l (t) + v r (t))/2.This leads to the conclusion that the motion of the mobile robot can be characterized by a nonlinear equation.
where s(t) represents the position of the mobile robot at time t, u(t) = [v(t), w(t)] T denotes the control input vector.
where l is two driving wheels wheelbase, v l (t) is the left-wheel speed, v r (t) is the right-wheel speed, v k,k is the angle between s k pose at moment k, the direction d k = [x k+1 -x k , y k+1 -y k , 0] T of the mobile robot.v k,k+1 is the angle between the d k direction and s k+1 posture of the mobile robot at time k + 1.When v k,k = v k,k +1 , a common arc of constant curvature can be obtained.
According to Fig. 3b, p k is the turning radius, arc length R k = p k ∆θ k .Angle ∆θ k = θ k+1 − θ k is the angle change between the s k position and s k+1 position of the mobile robot.The turning radius p k is.
where meet p k ≥ p min , p min is the minimum turning radius.

Traditional RRT* algorithm
The Rapidly Exploring Random Tree Star (RRT*) stands as a global path planning algorithm, representing an enhancement over the Rapidly Exploring Random Tree (RRT) 30 .The core method involves several steps: first, randomly sampling within the space.Once a sample point is obtained, the algorithm finds the nearby node.Second, a new node is created between this nearby node and the sampled one.Next, collision detection is done on the new node.If there's no collision, it's added to the path.The parent node is then adjusted for the new one.This continues until the algorithm reaches the target point.

Adaptive sampling function
In the conventional RRT* algorithm, the initial path is formed through random sampling across the entire obstacle space, resulting in a considerable number of sampling nodes.To enhance the target-oriented nature of sampling points, an adaptive sampling function is introduced.A proposed adaptive sampling function involves setting a target bias probability denoted as p. Generate sampling points based on the associated probability.This adaptive sampling function accelerates the generation of the initial path, reduces the quantity of sampling points, improves sampling efficiency, and prevents the initial path from succumbing to local optima.The adaptive sampling function is instrumental in achieving these enhancements.
where p represents probability, x rand denotes the sampling point generated by random sampling, X Goal signifies target point, X Goal is sampling point.

Node bias expansion
Given the relatively lengthy trajectory of the mobile robot, it is essential to constrain the expansion direction of the new node, denoted as X new .This constraint is crucial for expediting path planning 31 .In the traditional RRT* www.nature.com/scientificreports/algorithm, the new node X new is traditionally extended towards a randomly generated point, X rand .However, this random expansion approach often results in an unfocused expansion of nodes, leading to prolonged path planning durations.To address this inefficiency, this paper introduces a node expansion bias.This bias leverages target points to guide and constrain the expansion of new nodes.Specifically, different weights are assigned to the direction of the target point and the sampling point.The weight in the direction of the goal point, X Goal , is denoted as g, while the weight in the X rand direction is denoted as r.This configuration is illustrated in Fig. 4. The introduction of this node expansion bias aims to optimize the expansion process and enhance the efficiency of path planning.

Remove redundant points in the path
The path generated from the starting point to the destination point often contains numerous redundant points, contributing to the elongation of the path 32 .Reducing redundant points decreases the time needed for the mobile robot to complete the task, making it essential to trim them for a collision-free optimal path.This paper employs the greedy algorithm for path redundancy reduction.In Fig. 5, the blue line segment represents the branch of the random tree, the black line segment corresponds to the initial path, the red dotted line indicates the path after redundancy removal, and the gray circles symbolize obstacles.To remove redundancy path: Iterate through the path in turn.Until node X 4 is encountered, connecting nodes X 1 and X 4 will collide with obstacles.Connect X 1 and X 3 , delete node X 2 .Repeat the process until reaching node X 9 .The path with redundancy removed can be obtained: As shown in the Fig. 5.

Path smoothing
Once redundancy is eliminated, the path may still face issues with excessively wide turning angles.The path turning angle is too large, which does not conform to the kinematics of the mobile robot.Therefore, it is necessary to constrain the turning angle of the path.Firstly, the coordinates of three consecutive waypoints are obtained.The angular deviation of two adjacent coordinates is calculated.The angle deviation threshold A d is set.The calculated result is greater than the threshold.Then, the midpoint of the preceding.Following two points is used as the coordinates of the intermediate points, so as to achieve path smoothing.Secondly, the angle normalization function is set up.It combines angles from two nearby points in the map system into the range of (−π, π) to make angle comparisons consistent.Obtain the coordinates of the current time and the previous time waypoint.Calculate the distance.If the distance obtained is greater than the threshold, a new waypoint is inserted between the two waypoints.The coordinate is the point coordinate of the two path points, so as to increase the path point density.
Finally, the path smoothing function is established to obtain the coordinates of three adjacent waypoints.Then the angle value between two adjacent coordinates is calculated.The two angle standardization deviations are obtained by calculating and analyzing the angle standardization function.The calculation result is greater than the threshold.The middle coordinate points are substituted with the median value of the rear coordinate points.This process is repeated continuously to globally optimize the entire path, as shown in the Fig. 6.

Basic description of algorithm
The Timed Elastic Band (TEB) algorithm is a popular method for tackling local obstacle avoidance problems faced by mobile robots 33 .This algorithm is formally defined as a tuple comprising two sequences: where Q represents the sequence of robot poses, and τ signifies the sequence of time intervals associated with each pose.The basic idea is to dynamically adjust and optimize the algorithm with respect to configuration and time intervals.This is achieved through a real-time weighted multi-objective optimization process.
where B * refers to the optimized Timed Elastic Band (TEB) algorithm, where f k represents the weighted sum of components contributing to the objective function f(B).The objective function consists of four integral elements: (1) A penalty function, strategically employed to guide the intermediate points toward the original path and circumvent obstacles.(2) Utilization of dynamics to impose restrictions on the robot's speed and acceleration.
(3) Adherence to nonholonomic kinematic constraints.(4) Minimization of the square of the sum of the time difference sequence, aiming to identify the swiftest path.The TEB algorithm introduces a constraint on the robot's movement time, thereby enabling real-time control over the mobile robot's motion 34 .This additional constraint enhances the algorithm's ability to govern the movement of the mobile robot in dynamic environments 35 .

Algorithm optimization
The application of the TEB algorithm involves optimizing both the dynamics and kinematics of a mobile robot 36 .
The following is the optimization formula of TEB algorithm 37,38 .The main aim is to guide the robot smoothly from start to finish along the best path, considering dynamics and kinematics.The optimization problem is defined as a finite dimensional parameter vector (s k ) k=1,2,…,n−1 .consisting of a discretized sequence of n robot poses… n minus one.The parameter set to be optimized is:

Subject to
Among them, the initial pose s 1 and the end pose s n are replaced by the current mobile robot state s c and target state s f .Equality constraint h k (•)satisfies Eq. ( 5).r k (•)imposes a minimum turning radius and r k (•) = r k − p min .o is the simply connected area of obstacle model in obstacle map.When there are many obstacles, o will increase the subscript.For example, o l, l=1,…,n .ψ is an obstacle model.δ (s k , ψ) is the distance between the pose of the mobile robot and the obstacle model.The minimum distance of all obstacles is constrained by inequality δ min : where v k is the linear velocity of the robot at the pose s k .v k is defined as: where γ(•) ∈ [−1,1]is sign of mobile robot velocity.The result of mapping the direction vector q k = [ cosθ k , sinθ k , 0] T to the distance vector d k is: where < .,. > is the scalar computor.The angular velocity is w k = ∆θ k /∆T k and |w k |≤ w max the conditions are satisfied.w max = v max p −1 min .Linear acceleration is described as: where linear velocity and angular velocity constraints are The precise nonlinear programming is transformed into an approximate nonlinear square optimization problem.The optimization of the solution formula is achieved through the use of the approximate least squares method.The solver's properties are harnessed to approximate the first derivative, ensuring an effective solution.The equality constraint, denoted as h, is represented by the quadratic penalty with a scalar weight σ: The inequality approximates the weighted unilateral quadratic penalty: Equation ( 24) can be approximated by the overall unconstrained optimization of the objective function where B* is the optimal solution vector.When ownership values all approach infinity, B* coincides with the actual minimum value of Eq. ( 25).In order to solve (Eq.12), TEB algorithm adopts Levenberg Marquardt (LM) method.The graph optimization framework g2o implements an efficient sparse variant of LM.

Algorithm fusion
Combining the global and local path planning algorithms creates a hybrid algorithm.The global path planning involves multiple steps: firstly, employing an adaptive sampling function to generate random sampling points; secondly, generating a new node through node bias and adding it to the random tree; thirdly, removing redundant points in the initial path; fourthly, imposing constraints on the path turning angle and smoothing the path.The resultant global path information is then fed into the local path planning algorithm.The local path planning process consists of obtaining the path, applying kinematics constraints to the trajectory, verifying the trajectory, finally calculating the control inputs.This iterative process continues until the mobile robot reaches the target point, as depicted in Fig. 7.

Experimental
To evaluate the feasibility and effectiveness of the enhanced algorithm described in this paper, the Robot Operating System (ROS) serves as the operational environment.The simulation experiment takes place on a computer, and the prototype experiment is implemented on the mobile robot, enabling a thorough assessment.The model is constructed using Gazebo, a sophisticated robot simulation software renowned for its highfidelity physical simulation capabilities.Gazebo excels in accurately and efficiently replicating the intricate operations of robots within diverse indoor and outdoor settings.As a simulator, it swiftly validates the algorithm's effectiveness by creating a Gazebo model based on the experimental map.Following the construction of the simulation model, the next step involves employing the gmapping technique.This step is crucial for validating the improved RRT* algorithm's effectiveness.To thoroughly assess its performance, the A* algorithm, Voronoi algorithm, RRT algorithm, RRT connect algorithm, B spline smooth RRT connect algorithm, RRT* algorithm, B spline smooth RRT* algorithm, and the enhanced RRT* algorithm are individually executed within this mapped environment.The trajectory, depicted in Fig. 8, delineates specific elements: the gray area signifies safety, while the black cylinder represents obstacles, and the blue border denotes obstacle expansion.The mobile robot is denoted by the orange dot, and the red path illustrates the route planned by the global algorithm.Corresponding data is presented in Table 1 and Fig. 9. Notably, the path generated by the improved RRT* algorithm, as demonstrated in Fig. 8, exhibits a smoother trajectory compared to traditional algorithms.It significantly reduces the number of turning points, effectively enhancing the overall path quality.Further analysis, as evidenced in Table 1 and Fig. 9, reveals that compared to the traditional RRT* algorithm, the improved version reduces the path length by 5.8% and decreases the number of turning points by 62.5%.This reduction in path length enables the mobile robot to reach its destination more swiftly, while fewer waypoints conserve the robot's memory usage.
The resulting trajectory aligns with expectations.As illustrated in Fig. 10, The angular velocity and linear velocity of various RRT* algorithms applied to mobile robots are obtained and compared.In Fig. 10, the linear velocity consistently remains within the range of [−0.5 m/s, 1 m/s], The angular velocity also remains within the range of [-1 rad/s, 1 rad/s].Of particular note is that only the improved algorithm maintains a constant speed of 1 m/s between [5 s, 20 s], indicating a smooth operation of the mobile robot during this time interval.Compared www.nature.com/scientificreports/with other algorithms, the angular velocity fluctuation range of the improved algorithm is the smallest in the period of [5 s,18 s].These results confirm that the improved algorithm is more consistent with the kinematic model of the mobile robot, and can follow the planned path more effectively and move more smoothly.As can be seen from Fig. 11, in the environment of Fig. 8, the optimization convergence speed of the three algorithms is relatively close, but the Improved RRT * algorithm has the best curve, which can effectively improve the efficiency of path planning and reduce the time of path planning.
As can be seen from Fig. 12, the Improved RRT * is put into the map of three different environments for simulation experiments, and the Improved RRT * successfully generates the optimal path without colliding with obstacles.The results show that the Improved RRT * is relatively robust and can successfully plan paths and avoid obstacles in different environments.In Fig. 13, the influence of Angle deviation threshold A d on the number of path turns is analyzed, and it is found that setting the Angle at about π/20 degrees generates fewer path turns and smoother path.In Fig. 14, the relationship between the distance between the waypoints and the path length in the improved RRT * algorithm is analyzed.As shown in the figure, when the distance between the waypoints reaches 0.025 m, the impact on the distance is small, so 0.025 m is selected as the path point distance of the algorithm.
The hybrid algorithm is implemented in the mobile robot, and its effectiveness is verified through experimentation in a controlled environment, as illustrated in Fig. 15.Within the constructed environment, a hybrid algorithm is employed for path planning, and the outcome is depicted in Fig. 16.In Fig. 16b, point

Conclusion
Within this paper, introduce a fusion algorithm that synergizes the advancements of the improved RRT* algorithm and the TEB algorithm.The proposed approach not only expedites the generation of an optimal global path but also refines the trajectory, taking into careful consideration the dynamic constraints inherent to the mobile robot.The global path planning algorithm commences by employing an adaptive sampling function.Subsequently, Afterward, nodes expand with bias, redundant points are removed from the path, and turning angles are constrained.Perform kinematic modeling for the mobile robot.Incorporate dynamic constraints specific to the mobile robot into the local path planning algorithm, effectively integrating the algorithms.This holistic approach empowers the algorithm to strategize the mobile robot's path, leading to the attainment of a path of high quality.
In forthcoming research efforts, the emphasis transitions to utilizing a fusion algorithm for path planning within the framework of multiple robots.This necessitates addressing concerns such as priority assignment and potential conflicts in paths among the robots.The collaboration of multiple robots holds the promise of extending    www.nature.com/scientificreports/ the operational radius and enhancing overall efficiency.Concurrently, there is an examination of autonomously assigning execution tasks and areas to mobile robots.This strategy is designed to enhance robot efficiency and markedly improve the quality of work.Furthermore, the research delves into investigating how mobile robots navigate and avoid dynamic obstacles.

Figure 3 .
Figure 3. Mobile robot shape and posture diagram.
a designates the initial position of the mobile robot, point b represents the intermediate node during the robot's movement, and point c signifies the ultimate goal of the mobile robot.The purple path illustrates the global trajectory generated by the enhanced RRT* algorithm, while the red path depicts the local trajectory produced by the optimized

Table 1 .
Comparison of algorithm performance indexes before and after improvement.TEB algorithm.Using this algorithm for path planning guarantees a smooth trajectory that matches the robot's kinematic traits.Crucially, it avoids collisions with obstacles, achieving the intended result.